//*****************************************************************************************
//****************PROGRAMA PRINCIPAL PARA MANEJO DE PATA-RUEDA DE AMARANTA*****************
//*****************************************************************************************
// Autores: Ing. Carlos Felipe Santacruz   carlos.santacruz@javeriana.edu.co
//			Ing. Snaider Carrillo Lindao   snaidercl@gmail.com
//  ltima fecha de modificaci n: Julio 2008
// Descripci n: Manejo de pata rueda. Incluye:
//			    - Control de posici n para la direcci n de la rueda
//              - Control de velocidad para el giro de la rueda
//              - Manejo en malla abierta de flexi n-extensi n de la pata-rueda
//              - Comunicaci n serial con hyperterminal
//              - Comunicaci n por CAN con la tarjeta principal (PENDIENTE!!!!!)

//************COMENTARIOS HECHOS DE LA TARJETA POR ING. OTALORA***************
/*
CONFIGURACION PARA TARJETA: BASE30F6010

Tres controles de posici n.

Entradas analogas para 
potenciometros:								corriente:

RB8 - AN8	para pwm1						RB0 -  AN0 
RB9 - AN9	para pwm2						RB1 -  AN1
RB10- AN10	para pwm3						RB2 -  AN2
RB11- AN11	para pwm4.						RB3 - AN3

//  Camilo Otalora
//  Agosto-13-2005
//* Este programa control  para tres motores 		*
//* Entradas: poteciometros ANA0, ANA1, ANA2, ANA3			  	*	
//* El periodo de muestreo se fija con el timer 3.				*
//* Reloj principal de 10 Mhz para usar con PLLx8 				*
//* Se utiliza timer 1 para generar eventos 
//* transmisi n de datos por USART
*/

#include "p30F6010.h"
#include "amarantaUart.h"
#include "amarantaPwm.h"
#include "amarantaAdc.h"
#include "amarantaTimers.h"
#include "amarantaEncoders.h"
#include "pid.h"

#define MIN_ANGLE	75
#define MAX_ANGLE	300

//*****************************DEFINICI N DE CONSTANTES***************************
//----------------------------------------------------------DE AC 
#define NUM_ENCODER 1             // N mero de encoders en la pata (en total: 1)
// Constantes del controlador de posici n de direcci n de la rueda:
#define KP_POS 2800               // Constante de proporcionalidad
#define KI_POS 0.0                // Constante de integraci n
#define KD_POS 20                 // Constante de diferenciaci n
#define INT_MAX_POS   500         // Valor m ximo de la integral del error
#define INT_MIN_POS  -500         // Valor m nimo de la integral del error
#define ANG_INI 180                //  ngulo inicial (entero entre [])
// Constantes del controlador de velocidad de giro de la rueda:
#define KP_VEL 2600               // Constante de proporcionalidad
#define KI_VEL 3                 // Constante de integraci n
#define KD_VEL 10                 // Constante de diferenciaci n
#define INT_MAX_VEL   200         // Valor m ximo de la integral del error
#define INT_MIN_VEL  -200         // Valor m nimo de la integral del error
#define VEL_INI 0                  // Velocidad inicial (entero entre  [-100 100])

#define PII 3.1416 
#define TOL_ERROR 0.05                    //  Tolerancia del error para el control de posicin                       
//------------------------------------------------------------HASTA AC 




//*************************************RUTINAS**************************************
// Rutina de inicio de puertos:
void vSantaCruzInitPorts(void);				
void vSantaCruzMostrarResultados(void);



// *******************************VARIABLES GLOBALES******************************
// Variables para los pwms:
int t_SP;                    // Variable para manejar los PWMS desde el hyperterminal
//-------------------------------------------------------------------DE AC 
// Variables para el control:
float ref_ang, ref_ang_aux;         // Variable para almacenar la referencia del control de posici n desde el hyperterminal
float ref_vel, ref_vel_aux;         // Variable para almacenar la referencia del control de velocidad desde el hyperterminal
float ref_rod, ref_rod_aux;flag_rod;// Variable para almacenar el valor de comando del motor de la rodilla (en malla abierta)  
char stop;                          // Variable para parar todos los motores en caso de emergencia
s_pid pid_pos_rueda;                // Estructura de tipo s_pid para el control de posici n
s_pid pid_vel_rueda;                // Estructura de tipo s_pid para el control de velocidad

s_sensor xPotenciometro,xPosicion;		//Estructura tipo sensor para sensar la posicion, Estructura tipo sensor para establecer la referencia
float fPotenciometro, fPosicion;
double dToleranciaPosicion=TOL_ERROR;

s_sensor xEncoder,xVelocidad;			//Estructura tipo sensor para sensar la velocidad, Estructura tipo referencia para establecer la velocidad
float fEncoder, fVelocidad;

typedef struct appControl{
	
}

//*************************VARIABLES EXTERNAS DE OTROS ARCHIVOS********************
// Variables externas del m dulo de la UART:
extern static char volatile cAmarantaUartRxBufferIndex; 
extern static char volatile cAmarantaUartTxBufferIndex; 
extern char cAmarantaExecuteCommand;  // Variable que indica si se oprimi  un "Enter"

extern char pcAmarantaUartBufferRx[16];     // Buffer de recepcion
extern char pcAmarantaUartBufferTx[16];     // Buffer de transmision  
// Variables externas del m dulo ADC:
extern unsigned int uiAmarantaAdcResul, puiAmarantaAdcResultadoCanalAdc[4]; /* Bander que avisa de un nuevo resultado en el ADC - Buffer que almacena el resultado del ADC */

//-------------------------------------------------------------------DE AC 
// Variables externas del m dulo de los encoders:
extern unsigned int control_encoder[NUM_ENCODER];
extern float encoder_vel[NUM_ENCODER];
//-------------------------------------------------------------------HASTA AC 



int main(void)
{
	char i;
	
	// Inicializacion de puertos
	vAmarantaIoInitPort();		
	
	// Configuraci n de la USART1
	vAmarantaConfigUSART1();    
	
	// Inicializar los PWMS
	vAmarantaPwmInit();        
	
	// Inicializar el ADC
	vAmarantaAdcInit();         
	
	// Iniciar timers 1 y 2
	vAmarantaTimers(2000,50000,3);

	/* Inicializar el potenciometro y la referencia de posicion, junto con sus valores de normalizacion */
	vPidSensorCreate (&xPotenciometro,	&fPotenciometro,	1024.0,	PII, -512.0); 
	vPidSensorCreate (&xPosicion,		&fPosicion,		360.0,	PII, -180.0);
	/* Inicio de controlador de posici n de la direcci n de la rueda:*/
	dToleranciaPosicion=TOL_ERROR;
	vPidInit(&pid_pos_rueda, KP_POS, KI_POS, KD_POS, INT_MAX_POS, INT_MIN_POS,&xPotenciometro,&xPosicion,&dToleranciaPosicion);
	
	/* Inicializar el encoder y la referencia de velocidad, junto con sus valores de normalizacion */
	vPidSensorCreate (&xEncoder,	&fEncoder,	180.0,	PII, 		0.0); 
	vPidSensorCreate (&xVelocidad,	&fVelocidad,	18000.0,	PII*117.0,	0.0);
	
	/* Inicio de controlador de velocidad del giro de la rueda:*/
	vPidInit(&pid_vel_rueda, KP_VEL, KI_VEL, KD_VEL, INT_MAX_VEL, INT_MIN_VEL,&xEncoder,&xVelocidad,0x0);
	
	//Funcion de inicio de los Encoders.
	vAmarantaEncodersInit();  
	
	// Valores de referencia inicial para el control de posicion, velocidad y el comando para el motor de la rodilla:
	fPosicion = ANG_INI;      
	fVelocidad = VEL_INI;    
	ref_rod = 0; 
	flag_rod = 0;
	
	// Variable para para los motores en caso de emergencia, al comienzo es igual a cero:
	stop = 0;
	
	// Mensaje de texto inicial para indicar que el sistema est  bien:
	pcAmarantaUartBufferTx[0]='C';
	pcAmarantaUartBufferTx[1]='O';
	pcAmarantaUartBufferTx[2]='N';
	pcAmarantaUartBufferTx[3]='T';
	pcAmarantaUartBufferTx[4]='R';
	pcAmarantaUartBufferTx[5]='O';
	pcAmarantaUartBufferTx[6]='L';
	TxDat(7);
	
	//----------------------------------------------------HASTA AC 
	while(1)                           // Loop de polling
	{
		if(uiAmarantaAdcResul)	               // Entra cuando hay resultado en el ADC
			vSantaCruzMostrarResultados();
			
		//--------------------------------------------------------------DE AC 	
		if(cAmarantaExecuteCommand)
		{
			if(pcAmarantaUartBufferRx[0]=='P' && cAmarantaUartRxBufferIndex >= 7)        	// Manejo manual de los motores PNSXXXX, N motor=1,2,3,4, S signo
			{
				/* Convertir */
				t_SP = uiSantaCruzFixedAtoi(&pcAmarantaUartBuffer[3],4);
				if(pcAmarantaUartBufferRx[2]=='-')
					t_SP=-t_SP; 
				
				/* Comando Pwm */
				vAmarantaPwmSet(pcAmarantaUartBufferRx[1]&0x0f,t_SP);
			}
			if(pcAmarantaUartBufferRx[0]=='A' && pcAmarantaUartBufferRx[1]=='N' && pcAmarantaUartBufferRx[2]=='G' && cAmarantaUartRxBufferIndex>=5)        // ANGXXX   N motor=1,2  ;   XXX =[0,360)  
			{
				ref_ang_aux = uiSantaCruzFixedAtoi(pcAmarantaUartBufferRx[3],3);
								
				if (ref_ang_aux >= MIN_ANGLE && ref_ang_aux < MAX_ANGLE)     // Rangos permitidos para la direcci n de la llanta 
					fPosicion = ref_ang_aux;
				
									
					
			}
	
			if(pcAmarantaUartBufferRx[0]=='R' && pcAmarantaUartBufferRx[1]=='O' && pcAmarantaUartBufferRx[2]=='D' && cAmarantaUartRxBufferIndex>=6)        // RODSXXX   S= SIGNO  ;   XXX =[-100,100]  
			{
				ref_rod_aux = uiSantaCruzFixedAtoi(&pcAmarantaUartBufferRx[4],3);
				
				if(pcAmarantaUartBufferRx[3]=='-')
					ref_rod_aux = -ref_rod_aux;
				
				if (ref_rod_aux >= -100 && ref_rod_aux <= 100) 
					ref_rod=ref_rod_aux;   // Verificar si el comando esta en el rango permitido
					
				flag_rod = 1;              // Levantar la bandera que indica que hay comando
			}
	
	
			if(pcAmarantaUartBufferRx[0]=='V' && pcAmarantaUartBufferRx[1]=='E' && pcAmarantaUartBufferRx[2]=='L' && cAmarantaUartRxBufferIndex>=6)        // VELSXXX   S= SIGNO  ;   XXX =[-100,100]  
			{
				ref_vel_aux = uiSantaCruzFixedAtoi(&pcAmarantaUartBufferRx[4],3);
				
				if(pcAmarantaUartBufferRx[3]=='-')
					ref_vel_aux= -ref_vel_aux;
				
				if (ref_vel_aux >= -100 && ref_vel_aux <= 100) 
					ref_vel=ref_vel_aux;   //verificar si el nuevo angulo esta en el rango permitido
			}
			
			if (pcAmarantaUartBufferRx[0]=='E' && pcAmarantaUartBufferRx[1]=='N' && pcAmarantaUartBufferRx[2]=='C') // ENC   Devolver la velocidad del encoder en grados
			{
				
				TxDat(sprintf(pcAmarantaUartBufferTx,"ENC %05ud\n\r",control_encoder[0]));
			}
			if (pcAmarantaUartBufferRx[0]=='S')                     // Stop de emergencia
			{
				vAmarantaPwmSet(2, 0); 
				vAmarantaPwmSet(3, 0); 
				vAmarantaPwmSet(4, 0); 
				stop = 1;
			}
	
		 	cAmarantaUartRxBufferIndex=0;
		 	TxDat(sprintf(pcAmarantaUartBufferTx,"ok\n\r");
		 	cAmarantaExecuteCommand=0;
		}    

	}         
	
}



// Rutina de inicio de los puertos:
void vSantaCruzInitPorts(void)  // configuracion de I_O ports
{
	TRISD=0xffff;  // d0..3 entrada. 
	TRISE=0xfffe;  // RE[0] SALIDA QUE GENERA UNA SE AL CUADRADA AL ENTRAR UNA SENOIDAL ENTRE 1-50hz 		
}

//---------------------------------------------------------DE AC 
// Rutina que ejecuta el controlador PID cuando el ADC tiene datos:
void vSantaCruzMostrarResultados(void)
{
	vAmarantaEncoders();                             // C lcula la velocidad de la rueda
	if (stop == 0)                          // Si no hay emergencia se pueden mover los motores
	{
		vPidControlActions(&pid_pos_rueda, ref_ang, resultado_canal_adc[0], &pid_vel_rueda, ref_vel, encoder_vel[0]); // Ejecuta los controladores
		vAmarantaPwmSet(4, (signed int) (10*ref_rod));       // Se multiplica por 10 para hacer la transformaci n [-100 100]->[-1000 1000] 
		flag_rod = 0;
	}
	uiAmarantaAdcResul=0;							
	/* SE LIMPIA LA BANDERA DE NUEVO RESULTADO EN EL ADC  */
}

unsigned int uiSantaCruzFixedAtoi(char * b, int n){
	
	/* Convertir la cadena *b en entero sin signo */
	int retval=0,_n=0;
	
	while (_n<n){
		retval*=10;
		retval+=b[_n++]&0x0f;
	}
	return retval;
	
	
	
}

int iSantaCruzFixedAtoi(char * b, int n){

	/* Convertir la cadena b* en un entero con signo */
	int retval=0,sign=1,_n=0;
	
	/*  */		
	while (_n<n){
		if (b[_n]=='-' || b[_n]=='+' || b[_n]==' '){
			if (b[_n]=='-') 
				sign=-1;
		}
		else{
			retval*=10;
			retval+=b[_n]&0x0f;
		}
		_n++;
	}
		
	return retval*sign;

}
	


